/*
 * @brief: 三角电磁铁驱动
 * @date: 2015.09.09
 */
#include "em_cam.h"
#include "mhdef.h"
#include "dispatch.h"
#include "act_types.h"
#include "feedback.h"
#include <unistd.h>
#include <string.h>

/*
 * @param type: 动作类型
 * @param state: 0-3:左机头三角状态
 *               4-7:右机头三角状态
 */
int emcam_exec(MH_UDP *pudp, ActType type, uchar *state)
{
    CAN_MSG msg = { 0, 0, { 0 } };
    int i = 0;

    msg.id = (type == RESET) ? ID_CAM_RESET : ID_CAM;
    msg.len = 8;

    for (i = 0; i < 8; i++)
    {
        msg.data[i] = state[i];
    }

    return dispatch_msg(pudp, &msg, NULL);
}

/*
 * @param cur_state: 电磁铁当前状态
 * @param reset_state: 电磁铁复位状态
 * @param size: 数组长度
 * @note: 由于接线不同，有些机器复位之后，三角并不在零位，需要把某些不在零位的电磁铁置到零位状态
 */
int emcam_exec_reset_state(MH_UDP *pudp, uchar *cur_state, uchar *reset_state, int size)
{
    CAN_MSG msg = { 0, 0, { 0 } };
    int i = 0;
    int ret = 0;
    /* 状态相同 */
    if (memcmp(cur_state, reset_state, size) == 0)
    {
        return 0;
    }

    if (size > 8)
    {
        size = 8;
    }

    msg.id = ID_CAM;
    msg.len = size;

    memset(msg.data, 0, 8);
    memcpy(msg.data, cur_state, size);

    for (i = 0; i < size; i++)
    {
        FEEDBACK fb;
        msg.data[i] = reset_state[i];

        if (cur_state[i] == reset_state[i])
        {
            continue;
        }

        ret = dispatch_msg(pudp, &msg, NULL);
        if (ret < 0)
        {
            return ret;
        }

        usleep(3000);

        ret = feedback_get_error_state(pudp, &fb, 0);

        if (ret < 0)
        {
            return ret;
        }

        if (fb.error.cam)
        {
            return CRGERROR_CAM;
        }
    }

    return 0;
}
